import { subtract, length, normalize, cross } from './vec3';
//contains code from THREE.js

export function applyMatrix(out, v, e) {
    const x = v[0], y = v[1], z = v[2];
    // const e = in;

    const w = 1 / (e[ 3 ] * x + e[ 7 ] * y + e[ 11 ] * z + e[ 15 ]);

    out[0] = (e[ 0 ] * x + e[ 4 ] * y + e[ 8 ] * z + e[ 12 ]) * w;
    out[1] = (e[ 1 ] * x + e[ 5 ] * y + e[ 9 ] * z + e[ 13 ]) * w;
    out[2] = (e[ 2 ] * x + e[ 6 ] * y + e[ 10 ] * z + e[ 14 ]) * w;

    return out;
}

export function applyMatrix4(out, v, e) {
    const x = v[0], y = v[1], z = v[2], w = v[3];

    out[0] = e[ 0 ] * x + e[ 4 ] * y + e[ 8 ] * z + e[ 12 ] * w;
    out[1] = e[ 1 ] * x + e[ 5 ] * y + e[ 9 ] * z + e[ 13 ] * w;
    out[2] = e[ 2 ] * x + e[ 6 ] * y + e[ 10 ] * z + e[ 14 ] * w;
    out[3] = e[ 3 ] * x + e[ 7 ] * y + e[ 11 ] * z + e[ 15 ] * w;

    return this;

}

export function matrixToQuaternion(out, te) {
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

    // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)

    const m11 = te[ 0 ], m12 = te[ 4 ], m13 = te[ 8 ],
        m21 = te[ 1 ], m22 = te[ 5 ], m23 = te[ 9 ],
        m31 = te[ 2 ], m32 = te[ 6 ], m33 = te[ 10 ],

        trace = m11 + m22 + m33;
    let s;

    if (trace > 0) {

        s = 0.5 / Math.sqrt(trace + 1.0);

        out.w = 0.25 / s;
        out.x = (m32 - m23) * s;
        out.y = (m13 - m31) * s;
        out.z = (m21 - m12) * s;

    } else if (m11 > m22 && m11 > m33) {

        s = 2.0 * Math.sqrt(1.0 + m11 - m22 - m33);

        out.w = (m32 - m23) / s;
        out.x = 0.25 * s;
        out.y = (m12 + m21) / s;
        out.z = (m13 + m31) / s;

    } else if (m22 > m33) {

        s = 2.0 * Math.sqrt(1.0 + m22 - m11 - m33);

        out.w = (m13 - m31) / s;
        out.x = (m12 + m21) / s;
        out.y = 0.25 * s;
        out.z = (m23 + m32) / s;

    } else {

        s = 2.0 * Math.sqrt(1.0 + m33 - m11 - m22);

        out.w = (m21 - m12) / s;
        out.x = (m13 + m31) / s;
        out.y = (m23 + m32) / s;
        out.z = 0.25 * s;

    }

    return this;
}

export function quaternionToMatrix(out, q) {
    const te = out;

    const x = q.x, y = q.y, z = q.z, w = q.w;
    const x2 = x + x, y2 = y + y, z2 = z + z;
    const xx = x * x2, xy = x * y2, xz = x * z2;
    const yy = y * y2, yz = y * z2, zz = z * z2;
    const wx = w * x2, wy = w * y2, wz = w * z2;

    te[ 0 ] = 1 - (yy + zz);
    te[ 4 ] = xy - wz;
    te[ 8 ] = xz + wy;

    te[ 1 ] = xy + wz;
    te[ 5 ] = 1 - (xx + zz);
    te[ 9 ] = yz - wx;

    te[ 2 ] = xz - wy;
    te[ 6 ] = yz + wx;
    te[ 10 ] = 1 - (xx + yy);

    // last column
    te[ 3 ] = 0;
    te[ 7 ] = 0;
    te[ 11 ] = 0;

    // bottom row
    te[ 12 ] = 0;
    te[ 13 ] = 0;
    te[ 14 ] = 0;
    te[ 15 ] = 1;

    return te;
}

export function setPosition(out, v) {
    const te = out;

    te[ 12 ] = v[0];
    te[ 13 ] = v[1];
    te[ 14 ] = v[2];

    return out;
}

export function lookAt(te, eye, target, up) {
    const x = [0, 0, 0];
    const y = [0, 0, 0];
    const z = [0, 0, 0];
    subtract(z, eye, target);

    if (length(z) === 0) {

        // eye and target are in the same position

        z[2] = 1;

    }

    normalize(z, z);
    cross(x, up, z);

    if (length(z) === 0) {

        // up and z are parallel

        if (Math.abs(up[2]) === 1) {

            z[0] += 0.0001;

        } else {

            z[2] += 0.0001;

        }

        normalize(z, z);
        cross(x, up, z);

    }

    normalize(x, x);
    cross(y, z, x);

    te[ 0 ] = x[0]; te[ 4 ] = y[0]; te[ 8 ] = z[0];
    te[ 1 ] = x[1]; te[ 5 ] = y[1]; te[ 9 ] = z[1];
    te[ 2 ] = x[2]; te[ 6 ] = y[2]; te[ 10 ] = z[2];

    return te;
}
